﻿approach_azimuth = 180

parachute_deploy_altitude = 40

parachute_deploy_distance_from_home = 25

parachute_deploy_azimuth_from_home = 90

#DONT MESS WITH ANYTHING BELOW THIS LINE

import sys
import math
import clr
import time
import System
from System import Byte

clr.AddReference("MissionPlanner")
import MissionPlanner
clr.AddReference("MissionPlanner.Utilities") # includes the Utilities class
from MissionPlanner.Utilities import Locationwp
clr.AddReference("MAVLink") # includes the Utilities class
import MAVLink

idmavcmd = MAVLink.MAV_CMD.WAYPOINT
id = int(idmavcmd)

descent_ratio = 1.0/15.0 #1 meter descent for every 15 meters traveled
fly_thru_dist = 200

lathome = MAV.getWP(0).lat;
lnghome = MAV.getWP(0).lng;

current_alt = cs.alt
descent_delta = current_alt - parachute_deploy_altitude
descent_dist = .5*(descent_delta/descent_ratio)
alt_mid = (current_alt-parachute_deploy_altitude)/2+parachute_deploy_altitude

satellites = cs.satcount
hdop = cs.gpshdop

if satellites < 6:
	print'GPS FAILED.  NO MISSION CREATED. WAIT FOR BETTER GPS'
	print ''
elif hdop > 4:
	print'GPS FAILED.  NO MISSION CREATED. WAIT FOR BETTER GPS.'
	print ''
else:
	print'GPS PASSED.'
	print ''

	if descent_delta < 0:

		print 'CURRENT ALTITUDE BELOW PARACHUTE DEPLOY ALTITUDE'
		print 'NO MISSION CREATED'

	elif parachute_deploy_altitude < 20:

		print 'PARACHUTE DEPLOY ALTITUDE LESS THAN 20 METERS'
		print 'NO MISSION CREATED'

	elif parachute_deploy_altitude > 100:

		print 'PARACHUTE DEPLOY ALTITUDE MORE THAN 100 METERS'
		print 'NO MISSION CREATED'

	elif parachute_deploy_distance_from_home > 100:

		print 'PARACHUTE DEPLOY DISTANCE IS MORE THAN 100 FROM HOME'
		print 'NO MISSION CREATED'

	else:

		rad_earth = 6378100
		circ_earth = rad_earth*2*math.pi
		deglatpermeter = 360/circ_earth
	
		rad_lat = math.cos(lathome*2*math.pi/360)*rad_earth
		circ_lat = rad_lat*2*math.pi
		deglngpermeter = 360/circ_lat

		lat_delta2 = math.cos(parachute_deploy_azimuth_from_home*2*math.pi/360)*parachute_deploy_distance_from_home*deglatpermeter
		lng_delta2 = math.sin(parachute_deploy_azimuth_from_home*2*math.pi/360)*parachute_deploy_distance_from_home*deglngpermeter		

		lat2 = lathome + lat_delta2
		lng2 = lnghome + lng_delta2

		lat_delta1 = math.cos(approach_azimuth*2*math.pi/360)*descent_dist*deglatpermeter
		lng_delta1 = math.sin(approach_azimuth*2*math.pi/360)*descent_dist*deglngpermeter

		lat1 = lathome + lat_delta1 + lat_delta2
		lng1 = lnghome + lng_delta1 + lng_delta2

		lat_delta3 = math.cos((approach_azimuth+180)*2*math.pi/360)*fly_thru_dist*deglatpermeter
		lng_delta3 = math.sin((approach_azimuth+180)*2*math.pi/360)*fly_thru_dist*deglngpermeter		

		lat3 = lat2 + lat_delta3
		lng3 = lng2 + lng_delta3

		wp1 = Locationwp().Set(lathome,lnghome,current_alt, id)
		wp2 = Locationwp().Set(lat1,lng1,alt_mid, id)
		wp3 = Locationwp().Set(lat2,lng2,parachute_deploy_altitude, id)
		wp4 = Locationwp().Set(lat3,lng3,parachute_deploy_altitude, id)

		MAV.setWPTotal(5) #set wp total
		MAV.setWP(MAV.getWP(0),0,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT); #upload home - reset on arm
		MAV.setWP(wp1,1,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT); #upload wp1
		MAV.setWP(wp2,2,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT); #upload wp2
		MAV.setWP(wp3,3,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT); #upload wp3
		MAV.setWP(wp4,4,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT); #upload wp4

		MAV.setWPCurrent(1); #restart mission to waypoint 0
		Script.Sleep(1000)
		MAV.setWPACK(); #final ack
		Script.Sleep(1000)
		print 'AUTOGENERATED LANDING APPROACH MISSION'

		

